/*
 * IRobot.java
 *
 * Created on November 26, 2007, 2:57 PM
 *
 */

package core;

import java.awt.Color;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.Polygon;
import java.awt.geom.Point2D;
import java.util.Iterator;
import java.util.LinkedHashSet;
import java.util.Set;
import java.util.SortedSet;
import java.util.TreeSet;
import ui.AnimationCanvas;

/**
 *
 * @author Suraj Sapktoa [ ssapkota<at>gmail<dot>com ]
 * @version $Id:$
 */
public class IRobot implements Runnable{
    private AnimationCanvas ac=null;
    private boolean robotWalking;
    private Integer walkingSpeed;
    
    private int HEAD=0,
            NECK=1,
            BODY=2,
            HAND_LEFT_JOINT=3,
            HAND_RIGHT_JOINT=4,
            HAND_LEFT_UPPOR=5,
            HAND_LEFT_LOWER=6,
            HAND_RIGHT_UPPOR=7,
            HAND_RIGHT_LOWER=8,
            LEG_LEFT_UPPOR=9,
            LEG_LEFT_LOWER=10,
            LEG_RIGHT_UPPOR=11,
            LEG_RIGHT_LOWER=12,
            FOOT_LEFT=13,
            FOOT_RIGHT=14;
    private String[] bodyParts={"Head", "Neck", "Body", "Left Hand Joint", "Right Hand Joint", "Uppor Left Hand","Lower Left Hand","Uppor Right Hand",
    "Lower Right Hand", "Uppor Left Leg", "Lower Left Leg", "Uppor Right Leg", "Lower Right Leg","Left Foot", "Right Foot"};
    
    //points of cube
    private int F0=0,F1=1,F2=2,F3=3,                 //pts of Frontface
            B0=4,B1=5,B2=6,B3=7,                 //Points of Backface
            FC=8,BaC=9,TC=10,BtC=11,LC=12,RC=13, //Centres of respective faces (F(ront),Ba(ck),T(op),B(o)t(tom),L(eft),R(ight))
            CC=14;                               //Component centre
    private Point3D[][] Points=null;
    //extra points to determine painting order of components
    private Point3D[] extraPoints;
    private Point3D[][] FacePoints;
    
    
    private int COMPONENT_COUNT=15;
    private SortedSet<Component> robot=null;
    private Component[] component=null;
    private int Front=0,Back=1,Top=2,Bottom=3,Left=4,Right=5;
    
    int CurrentFrame =-1;
    
    private Frame[] frames=null;
    private boolean destroyRobot;
    
    /**
     * Creates a new instance of IRobot
     */
    public IRobot(AnimationCanvas ac) {
        this.ac=ac;
        destroyRobot = false;
        robotWalking = false;
        walkingSpeed = 300;
        robot=new TreeSet<Component>();
        component=new Component[COMPONENT_COUNT];
        initRobot();
        initWalk();
    }
    public void paint(Graphics2D g2){
        this.paint(g2,CurrentFrame);
    }
    public void paint(Graphics2D g2,int frame){
        int i=0;
        //Step 1: Reset the moved and projected value of each point
        for(i=0;i<this.COMPONENT_COUNT;i++){
            for(Point3D p: this.Points[i]){
                p.setMoved(false);
                p.setProjected(false);
            }
        }
        for(i=0;i<this.FacePoints.length;i++){
            for(Point3D p: this.FacePoints[i]){
                p.setMoved(false);
                p.setProjected(false);
            }
        }
        for(Point3D p: extraPoints){
            p.setMoved(false);
            p.setProjected(false);
        }
        
        //Step 2: Move each Component/Point if required
        //There are many points shared by two components. They are Moved only once
        // The Point3D.move() function will manage it.
        if(CurrentFrame!=frame){
            CurrentFrame++;
            CurrentFrame%=frames.length;
            for(i=0;i<this.extraPoints.length;i++){
                this.extraPoints[i].move(frames[CurrentFrame].getDx());
            }
            for(i=0;i<this.FacePoints.length;i++){
                for(Point3D p: this.FacePoints[i]){
                    p.move(frames[CurrentFrame].getDx());
                }
            }
            for(i=0;i<this.COMPONENT_COUNT;i++){
                component[i].move(frames[CurrentFrame].getDx());
                //System.out.println("Projecting :"+bodyParts[i]);
                for(Point3D p: this.Points[i]){
                    p.move(frames[CurrentFrame].getDx());
                }
            }
            //rotate the hands and legs
            //reset the moved value
            for(Point3D p: Points[HAND_LEFT_UPPOR]){
                p.setMoved(false);
            }
            for(Point3D p: Points[HAND_LEFT_LOWER]){
                p.setMoved(false);
            }
            for(Point3D p: Points[HAND_RIGHT_UPPOR]){
                p.setMoved(false);
            }
            for(Point3D p: Points[HAND_RIGHT_LOWER]){
                p.setMoved(false);
            }
            for(Point3D p: Points[LEG_LEFT_UPPOR]){
                p.setMoved(false);
            }
            for(Point3D p: Points[LEG_LEFT_LOWER]){
                p.setMoved(false);
            }
            for(Point3D p: Points[LEG_RIGHT_UPPOR]){
                p.setMoved(false);
            }
            for(Point3D p: Points[LEG_RIGHT_LOWER]){
                p.setMoved(false);
            }
            
            //rotate the hands and legs
            
            //rotate left hand
            this.RotateY(Points[HAND_LEFT_UPPOR],frames[CurrentFrame].getHAng11(),Points[HAND_LEFT_JOINT][CC]);
            this.RotateY(Points[HAND_LEFT_LOWER],frames[CurrentFrame].getHAng11(),Points[HAND_LEFT_JOINT][CC]);
            //rotate Right hand
            this.RotateY(Points[HAND_RIGHT_UPPOR],frames[CurrentFrame].getHAng21(),Points[HAND_RIGHT_JOINT][CC]);
            this.RotateY(Points[HAND_RIGHT_LOWER],frames[CurrentFrame].getHAng21(),Points[HAND_RIGHT_JOINT][CC]);
            
            //again rotate lower hands withrespect to corresponding uppor hands
            //reset moved of the points that are to be moved
            for (int j = 0; j < Points[HAND_LEFT_LOWER].length; j++) {
                if(j!=F0 && j!=F1 && j!=B0 && j!=B1 && j!=TC){
                    Points[HAND_LEFT_LOWER][j].setMoved(false);
                    Points[HAND_RIGHT_LOWER][j].setMoved(false);
                }
            }
            //rotate the lower hands
            this.RotateY(Points[HAND_LEFT_LOWER],frames[CurrentFrame].getHAng12(),Points[HAND_LEFT_UPPOR][B3]);
            this.RotateY(Points[HAND_RIGHT_LOWER],frames[CurrentFrame].getHAng22(),Points[HAND_RIGHT_UPPOR][B3]);
            
            
            //rotate the legs
            float dxl=-Points[LEG_LEFT_LOWER][B3].getX(),dzl=-Points[LEG_LEFT_LOWER][B3].getZ();
            float dxr=-Points[LEG_RIGHT_LOWER][B3].getX(),dzr=-Points[LEG_RIGHT_LOWER][B3].getZ();
            for (int j = 0; j < Points[LEG_LEFT_UPPOR].length; j++) {
                if(j==F2 || j==F3 || j==FC){
                    this.RotateY(Points[LEG_LEFT_UPPOR][j],frames[CurrentFrame].getLAng11(),Points[LEG_LEFT_UPPOR][F0]);
                    this.RotateY(Points[LEG_LEFT_LOWER][j],frames[CurrentFrame].getLAng11(),Points[LEG_LEFT_UPPOR][F0]);
                    this.RotateY(Points[LEG_RIGHT_UPPOR][j],frames[CurrentFrame].getLAng21(),Points[LEG_RIGHT_UPPOR][F0]);
                    this.RotateY(Points[LEG_RIGHT_LOWER][j],frames[CurrentFrame].getLAng21(),Points[LEG_RIGHT_UPPOR][F0]);
                    
                    Points[LEG_LEFT_LOWER][j].setMoved(false);
                    Points[LEG_RIGHT_LOWER][j].setMoved(false);
                    this.RotateY(Points[LEG_LEFT_LOWER][j],frames[CurrentFrame].getLAng12(),Points[LEG_LEFT_LOWER][F1]);
                    this.RotateY(Points[LEG_RIGHT_LOWER][j],frames[CurrentFrame].getLAng22(),Points[LEG_RIGHT_LOWER][F1]);
                    
                } else if(j==B2 || j==B3 || j==BaC){
                    this.RotateY(Points[LEG_LEFT_UPPOR][j],frames[CurrentFrame].getLAng11(),Points[LEG_LEFT_UPPOR][B0]);
                    this.RotateY(Points[LEG_LEFT_LOWER][j],frames[CurrentFrame].getLAng11(),Points[LEG_LEFT_UPPOR][B0]);
                    this.RotateY(Points[LEG_RIGHT_UPPOR][j],frames[CurrentFrame].getLAng21(),Points[LEG_RIGHT_UPPOR][B0]);
                    this.RotateY(Points[LEG_RIGHT_LOWER][j],frames[CurrentFrame].getLAng21(),Points[LEG_RIGHT_UPPOR][B0]);
                    
                    Points[LEG_LEFT_LOWER][j].setMoved(false);
                    Points[LEG_RIGHT_LOWER][j].setMoved(false);
                    this.RotateY(Points[LEG_LEFT_LOWER][j],frames[CurrentFrame].getLAng12(),Points[LEG_LEFT_LOWER][B1]);
                    this.RotateY(Points[LEG_RIGHT_LOWER][j],frames[CurrentFrame].getLAng22(),Points[LEG_RIGHT_LOWER][B1]);
                } else if(j==RC || j==LC || j==CC || j==BtC){
                    this.RotateY(Points[LEG_LEFT_UPPOR][j],frames[CurrentFrame].getLAng11(),Points[LEG_LEFT_UPPOR][TC]);
                    this.RotateY(Points[LEG_LEFT_LOWER][j],frames[CurrentFrame].getLAng11(),Points[LEG_LEFT_UPPOR][TC]);
                    this.RotateY(Points[LEG_RIGHT_UPPOR][j],frames[CurrentFrame].getLAng21(),Points[LEG_RIGHT_UPPOR][TC]);
                    this.RotateY(Points[LEG_RIGHT_LOWER][j],frames[CurrentFrame].getLAng21(),Points[LEG_RIGHT_UPPOR][TC]);
                    
                    Points[LEG_LEFT_LOWER][j].setMoved(false);
                    Points[LEG_RIGHT_LOWER][j].setMoved(false);
                    this.RotateY(Points[LEG_LEFT_LOWER][j],frames[CurrentFrame].getLAng12(),Points[LEG_LEFT_LOWER][TC]);
                    this.RotateY(Points[LEG_RIGHT_LOWER][j],frames[CurrentFrame].getLAng22(),Points[LEG_RIGHT_LOWER][TC]);
                }
            }
            
            dxl+=Points[LEG_LEFT_LOWER][B3].getX();
            dzl+=Points[LEG_LEFT_LOWER][B3].getZ();
            dxr+=Points[LEG_RIGHT_LOWER][B3].getX();
            dzr+=Points[LEG_RIGHT_LOWER][B3].getZ();
            //System.out.println("dxr ="+dxr+" dzr ="+dzr);
            //move foot too
            for(Point3D p: this.Points[FOOT_LEFT]){
                p.setMoved(false);
                p.move(dxl,dzl);
            }
            for(Point3D p: this.Points[FOOT_RIGHT]){
                p.setMoved(false);
                p.move(dxr,dzr);
            }
        }
        
        //Step 3: Project each Point
        //There are many points shared by two components. They are projected only once
        // The Point3D.project() function will manage it.
        for(i=0;i<this.COMPONENT_COUNT;i++){
            //System.out.println("Projecting :"+bodyParts[i]);
            for(Point3D p: this.Points[i]){
                p.project();
            }
        }
        for(i=0;i<this.FacePoints.length;i++){
            for(Point3D p: this.FacePoints[i]){
                p.project();
            }
        }
        for(Point3D p: extraPoints){
            p.project();
        }
        
        //Step:4 Compute new distance of each component from eye.
        for(Component c: component){
            c.computeDistance();
        }
        Iterator iter=robot.iterator();
        SortedSet<Component> robot1=new TreeSet<Component>();
        //Steo: 5 Shorting the components in the order of there distance
        //Tree set has its efficient internal sorting algorithm. It uses the Component.compareTo() function.
        while(iter.hasNext()){
            Component c=((Component)iter.next());
            robot1.add(c);
        }
        this.robot=robot1;
        
        //Step: 6 Finally paint each components
        iter=robot.iterator();
        while(iter.hasNext()){
            Component comp=((Component)iter.next());
            if (comp.getName().equals(bodyParts[HAND_LEFT_UPPOR])){
                if(this.component[HAND_LEFT_LOWER].compareTo(this.component[HAND_LEFT_UPPOR])<0){
                    this.component[HAND_LEFT_LOWER].paint(g2);
                    this.component[HAND_LEFT_UPPOR].paint(g2);
                } else{
                    this.component[HAND_LEFT_UPPOR].paint(g2);
                    this.component[HAND_LEFT_LOWER].paint(g2);
                }
            } else if (comp.getName().equals(bodyParts[HAND_RIGHT_UPPOR])){
                if(this.component[HAND_RIGHT_LOWER].compareTo(this.component[HAND_RIGHT_UPPOR])<0){
                    this.component[HAND_RIGHT_LOWER].paint(g2);
                    this.component[HAND_RIGHT_UPPOR].paint(g2);
                } else{
                    this.component[HAND_RIGHT_UPPOR].paint(g2);
                    this.component[HAND_RIGHT_LOWER].paint(g2);
                }
            } else if (comp.getName().equals(bodyParts[LEG_LEFT_UPPOR])){
                if(this.component[LEG_LEFT_LOWER].compareTo(this.component[LEG_LEFT_UPPOR])<0){
                    this.component[LEG_LEFT_LOWER].paint(g2);
                    this.component[LEG_LEFT_UPPOR].paint(g2);
                } else{
                    this.component[LEG_LEFT_UPPOR].paint(g2);
                    this.component[LEG_LEFT_LOWER].paint(g2);
                }
            } else if (comp.getName().equals(bodyParts[LEG_RIGHT_UPPOR])){
                if(this.component[LEG_RIGHT_LOWER].compareTo(this.component[LEG_RIGHT_UPPOR])<0){
                    this.component[LEG_RIGHT_LOWER].paint(g2);
                    this.component[LEG_RIGHT_UPPOR].paint(g2);
                } else{
                    this.component[LEG_RIGHT_UPPOR].paint(g2);
                    this.component[LEG_RIGHT_LOWER].paint(g2);
                }
            } else{
                comp.paint(g2);
            }
        }
    }
    
    /**Initilize the Robot in world coordinate
     *The coordinte system is assumed as follows
     *
     *              ^ Z (Head)
     *              |
     *              |
     *              |
     *              +------------> X (Front)
     *             /
     *            /
     *           /
     *          \/
     *        Y (Right)
     */
    private void initRobot(){
        // component point
        robot.clear();
        Set<Surface> comp=null;
        LinkedHashSet<Point3D> surface=null;
        Point3D[] pts=null;
        extraPoints=new Point3D[3];
        
        this.Points=new Point3D[COMPONENT_COUNT][];
        Surface[][] surfaces=new Surface[COMPONENT_COUNT][];
        //  The naming of each component is based upon following convension.
        //
        //  4 ________ 5
        //   |\        \
        //   | \________\ 1
        //   |  | 0      |
        //   |  |        |
        //   |  |        |  <--6 (hidden)
        //  7 \ |        |
        //     \|________|
        //     3          2
        //
        // All the surfaces are defined in anticlockwise order of vertices.
        //
        
        Point3D centre=null;
        int w=0,h=0,b=0;
        
        for(int i=0;i<COMPONENT_COUNT;i++){
            //this must always be linked hash set --> this maintains the insersion order
            comp=new LinkedHashSet<Surface>();
            surfaces[i]=new Surface[6];
            pts=new Point3D[15];
            System.out.print("Initilizing "+bodyParts[i]+"...");
            
            if(i==this.HEAD){
                centre=new Point3D(ac,0,0,Constants.TOTAL_BODY_HEIGHT-Constants.HEAD_HEIGHT/2);
                pts[CC]=centre;
                w=Constants.HEAD_WIDTH;
                h=Constants.HEAD_HEIGHT;
                b=Constants.HEAD_BREDTH;
            } else if(i==this.NECK){
                centre=new Point3D(ac,0,0,Constants.TOTAL_BODY_HEIGHT-Constants.HEAD_HEIGHT-Constants.NECK_HEIGHT/2);
                pts[CC]=centre;
                w=Constants.NECK_WIDTH;
                h=Constants.NECK_HEIGHT;
                b=Constants.NECK_BREDTH;
            } else if(i==this.BODY){
                centre=new Point3D(ac,0,0,Constants.TOTAL_BODY_HEIGHT-Constants.HEAD_HEIGHT-Constants.NECK_HEIGHT-Constants.BODY_HEIGHT/2);
                pts[CC]=centre;
                extraPoints[0]=new Point3D(ac,centre.getX(),centre.getY(),(float)(centre.getZ()+Constants.BODY_HEIGHT*0.4));
                
                w=Constants.BODY_WIDTH;
                h=Constants.BODY_HEIGHT;
                b=Constants.BODY_BREDTH;
            } else if(i==this.HAND_LEFT_JOINT){
                centre=new Point3D(ac,0,-(Constants.BODY_BREDTH+Constants.HAND_JOINT_BREDTH)/2,Constants.TOTAL_BODY_HEIGHT-Constants.HEAD_HEIGHT-Constants.NECK_HEIGHT-Constants.HAND_JOINT_HEIGHT/2-Constants.BODY_HEIGHT/10);
                pts[CC]=centre;
                w=Constants.HAND_JOINT_WIDTH;
                h=Constants.HAND_JOINT_HEIGHT;
                b=Constants.HAND_JOINT_BREDTH;
            } else if(i==this.HAND_RIGHT_JOINT){
                centre=new Point3D(ac,0,(Constants.BODY_BREDTH+Constants.HAND_JOINT_BREDTH)/2,Constants.TOTAL_BODY_HEIGHT-Constants.HEAD_HEIGHT-Constants.NECK_HEIGHT-Constants.HAND_JOINT_HEIGHT/2-Constants.BODY_HEIGHT/10);
                pts[CC]=centre;
                w=Constants.HAND_JOINT_WIDTH;
                h=Constants.HAND_JOINT_HEIGHT;
                b=Constants.HAND_JOINT_BREDTH;
            } else if(i==this.HAND_LEFT_UPPOR){
                centre=new Point3D(ac,0,-(Constants.BODY_BREDTH+Constants.HAND_BREDTH)/2-Constants.HAND_JOINT_BREDTH,Constants.TOTAL_BODY_HEIGHT-Constants.HEAD_HEIGHT-Constants.NECK_HEIGHT-Constants.HAND_UPPOR_HEIGHT/2);
                pts[CC]=centre;
                extraPoints[1]=new Point3D(ac,centre.getX(),centre.getY(),(float)(centre.getZ()+Constants.HAND_UPPOR_HEIGHT/2-Constants.BODY_HEIGHT/10));
                
                w=Constants.HAND_WIDTH;
                h=Constants.HAND_UPPOR_HEIGHT;
                b=Constants.HAND_BREDTH;
            } else if(i==this.HAND_RIGHT_UPPOR){
                centre=new Point3D(ac,0,(Constants.BODY_BREDTH+Constants.HAND_BREDTH)/2+Constants.HAND_JOINT_BREDTH,Constants.TOTAL_BODY_HEIGHT-Constants.HEAD_HEIGHT-Constants.NECK_HEIGHT-Constants.HAND_UPPOR_HEIGHT/2);
                pts[CC]=centre;
                extraPoints[2]=new Point3D(ac,centre.getX(),centre.getY(),(float)(centre.getZ()+Constants.HAND_UPPOR_HEIGHT/2-Constants.BODY_HEIGHT/10));
                
                w=Constants.HAND_WIDTH;
                h=Constants.HAND_UPPOR_HEIGHT;
                b=Constants.HAND_BREDTH;
            } else if(i==this.HAND_LEFT_LOWER){
                centre=new Point3D(ac,0,-(Constants.BODY_BREDTH+Constants.HAND_BREDTH)/2-Constants.HAND_JOINT_BREDTH,Constants.TOTAL_BODY_HEIGHT-Constants.HEAD_HEIGHT-Constants.NECK_HEIGHT-Constants.HAND_UPPOR_HEIGHT-Constants.HAND_LOWER_HEIGHT/2);
                pts[CC]=centre;
                w=Constants.HAND_WIDTH;
                h=Constants.HAND_LOWER_HEIGHT;
                b=Constants.HAND_BREDTH;
            } else if(i==this.HAND_RIGHT_LOWER){
                centre=new Point3D(ac,0,(Constants.BODY_BREDTH+Constants.HAND_BREDTH)/2+Constants.HAND_JOINT_BREDTH,Constants.TOTAL_BODY_HEIGHT-Constants.HEAD_HEIGHT-Constants.NECK_HEIGHT-Constants.HAND_UPPOR_HEIGHT-Constants.HAND_LOWER_HEIGHT/2);
                pts[CC]=centre;
                w=Constants.HAND_WIDTH;
                h=Constants.HAND_LOWER_HEIGHT;
                b=Constants.HAND_BREDTH;
            } else if(i==this.LEG_LEFT_UPPOR){
                centre=new Point3D(ac,0,-(Constants.LEG_GAP+Constants.LEG_BREDTH)/2,Constants.FOOT_HEIGHT+Constants.LEG_LOWER_HEIGHT+Constants.LEG_UPPOR_HEIGHT/2);
                pts[CC]=centre;
                w=Constants.LEG_WIDTH;
                h=Constants.LEG_UPPOR_HEIGHT;
                b=Constants.LEG_BREDTH;
            } else if(i==this.LEG_RIGHT_UPPOR){
                centre=new Point3D(ac,0,(Constants.LEG_GAP+Constants.LEG_BREDTH)/2,Constants.FOOT_HEIGHT+Constants.LEG_LOWER_HEIGHT+Constants.LEG_UPPOR_HEIGHT/2);
                pts[CC]=centre;
                w=Constants.LEG_WIDTH;
                h=Constants.LEG_UPPOR_HEIGHT;
                b=Constants.LEG_BREDTH;
            } else if(i==this.LEG_LEFT_LOWER){
                centre=new Point3D(ac,0,-(Constants.LEG_GAP+Constants.LEG_BREDTH)/2,Constants.FOOT_HEIGHT+Constants.LEG_LOWER_HEIGHT/2);
                pts[CC]=centre;
                w=Constants.LEG_WIDTH;
                h=Constants.LEG_LOWER_HEIGHT;
                b=Constants.LEG_BREDTH;
            } else if(i==this.LEG_RIGHT_LOWER){
                centre=new Point3D(ac,0,(Constants.LEG_GAP+Constants.LEG_BREDTH)/2,Constants.FOOT_HEIGHT+Constants.LEG_LOWER_HEIGHT/2);
                pts[CC]=centre;
                w=Constants.LEG_WIDTH;
                h=Constants.LEG_LOWER_HEIGHT;
                b=Constants.LEG_BREDTH;
            } else if(i==this.FOOT_LEFT){
                centre=new Point3D(ac,((Constants.FOOT_WIDTH-Constants.LEG_WIDTH)/2),-(Constants.LEG_GAP+Constants.LEG_BREDTH)/2,Constants.FOOT_HEIGHT/2);
                pts[CC]=centre;
                w=Constants.FOOT_WIDTH;
                h=Constants.FOOT_HEIGHT;
                b=Constants.FOOT_BREDTH;
            } else if(i==this.FOOT_RIGHT){
                centre=new Point3D(ac,((Constants.FOOT_WIDTH-Constants.LEG_WIDTH)/2),(Constants.LEG_GAP+Constants.LEG_BREDTH)/2,Constants.FOOT_HEIGHT/2);
                pts[CC]=centre;
                w=Constants.FOOT_WIDTH;
                h=Constants.FOOT_HEIGHT;
                b=Constants.FOOT_BREDTH;
            }
            
            System.out.print("Centre: "+centre);
            pts[F0]=new Point3D(ac,(int)centre.getX()+w/2,(int)centre.getY()+b/2,(int)centre.getZ()+h/2);
            pts[F1]=new Point3D(ac,(int)centre.getX()+w/2,(int)centre.getY()-b/2,(int)centre.getZ()+h/2);
            pts[F2]=new Point3D(ac,(int)centre.getX()+w/2,(int)centre.getY()-b/2,(int)centre.getZ()-h/2);
            pts[F3]=new Point3D(ac,(int)centre.getX()+w/2,(int)centre.getY()+b/2,(int)centre.getZ()-h/2);
            pts[B0]=new Point3D(ac,(int)centre.getX()-w/2,(int)centre.getY()+b/2,(int)centre.getZ()+h/2);
            pts[B1]=new Point3D(ac,(int)centre.getX()-w/2,(int)centre.getY()-b/2,(int)centre.getZ()+h/2);
            pts[B2]=new Point3D(ac,(int)centre.getX()-w/2,(int)centre.getY()-b/2,(int)centre.getZ()-h/2);
            pts[B3]=new Point3D(ac,(int)centre.getX()-w/2,(int)centre.getY()+b/2,(int)centre.getZ()-h/2);
            
            //if its either of lower hands/ lower legs then attach it to its corresponding uppor part
            if(i==this.HAND_LEFT_LOWER){
                pts[F0]=this.Points[HAND_LEFT_UPPOR][F3];
                pts[F1]=this.Points[HAND_LEFT_UPPOR][F2];
                pts[B1]=this.Points[HAND_LEFT_UPPOR][B2];
                pts[B0]=this.Points[HAND_LEFT_UPPOR][B3];
            } else if(i==this.HAND_RIGHT_LOWER){
                pts[F0]=this.Points[HAND_RIGHT_UPPOR][F3];
                pts[F1]=this.Points[HAND_RIGHT_UPPOR][F2];
                pts[B1]=this.Points[HAND_RIGHT_UPPOR][B2];
                pts[B0]=this.Points[HAND_RIGHT_UPPOR][B3];
            } else if(i==this.LEG_LEFT_LOWER){
                pts[F0]=this.Points[LEG_LEFT_UPPOR][F3];
                pts[F1]=this.Points[LEG_LEFT_UPPOR][F2];
                pts[B1]=this.Points[LEG_LEFT_UPPOR][B2];
                pts[B0]=this.Points[LEG_LEFT_UPPOR][B3];
            } else if(i==this.LEG_RIGHT_LOWER){
                pts[F0]=this.Points[LEG_RIGHT_UPPOR][F3];
                pts[F1]=this.Points[LEG_RIGHT_UPPOR][F2];
                pts[B1]=this.Points[LEG_RIGHT_UPPOR][B2];
                pts[B0]=this.Points[LEG_RIGHT_UPPOR][B3];
            }
            
            //front face
            //this must always be linked hash set --> this maintains the insersion order
            surface=new LinkedHashSet<Point3D>();
            surface.add(pts[F0]);
            surface.add(pts[F3]);
            surface.add(pts[F2]);
            surface.add(pts[F1]);
            pts[FC]=new Point3D(ac,pts[F0].getX(),(pts[F0].getY()+pts[F1].getY())/2,(pts[F0].getZ()+pts[F3].getZ())/2);
            comp.add(surfaces[i][this.Front]=new Surface("Front Face",ac,surface,pts[FC]));
            
            //back face
            surface=new LinkedHashSet<Point3D>();
            surface.add(pts[B0]);
            surface.add(pts[B1]);
            surface.add(pts[B2]);
            surface.add(pts[B3]);
            pts[BaC]=new Point3D(ac,pts[B0].getX(),(pts[B0].getY()+pts[B1].getY())/2,(pts[B0].getZ()+pts[B3].getZ())/2);
            comp.add(surfaces[i][this.Back]=new Surface("Back Face",ac,surface,pts[BaC]));
            
            //Top face
            surface=new LinkedHashSet<Point3D>();
            surface.add(pts[F0]);
            surface.add(pts[F1]);
            surface.add(pts[B1]);
            surface.add(pts[B0]);
            pts[TC]=new Point3D(ac,(pts[F0].getX()+pts[B0].getX())/2,(pts[F0].getY()+pts[F1].getY())/2,pts[F0].getZ());
            comp.add(surfaces[i][this.Top]=new Surface("Top Face",ac,surface,pts[TC]));
            
            
            //Bottom face
            surface=new LinkedHashSet<Point3D>();
            surface.add(pts[F3]);
            surface.add(pts[B3]);
            surface.add(pts[B2]);
            surface.add(pts[F2]);
            pts[BtC]=new Point3D(ac,(pts[F3].getX()+pts[B3].getX())/2,(pts[F3].getY()+pts[2].getY())/2,pts[F3].getZ());
            comp.add(surfaces[i][this.Bottom]=new Surface("Bottom Face",ac,surface,pts[BtC]));
            
            //Left face
            surface=new LinkedHashSet<Point3D>();
            surface.add(pts[F1]);
            surface.add(pts[F2]);
            surface.add(pts[B2]);
            surface.add(pts[B1]);
            pts[LC]=new Point3D(ac,(pts[F1].getX()+pts[B1].getX())/2,pts[F1].getY(),(pts[F1].getZ()+pts[F2].getZ())/2);
            comp.add(surfaces[i][this.Left]=new Surface("Left Face",ac,surface,pts[LC]));
            
            //Right face
            surface=new LinkedHashSet<Point3D>();
            surface.add(pts[F0]);
            surface.add(pts[B0]);
            surface.add(pts[B3]);
            surface.add(pts[F3]);
            pts[RC]=new Point3D(ac,(pts[F0].getX()+pts[B0].getX())/2,pts[F0].getY(),(pts[F0].getZ()+pts[F3].getZ())/2);
            comp.add(surfaces[i][this.Right]=new Surface("Right Face",ac,surface,pts[RC]));
            
            
            if(i==BODY){
                component[i]=new Component(bodyParts[i],ac,comp,centre,extraPoints[0]);
            } else if(i==HAND_LEFT_UPPOR){
                component[i]=new Component(bodyParts[i],ac,comp,centre,extraPoints[1]);
            } else if(i==HAND_RIGHT_UPPOR){
                component[i]=new Component(bodyParts[i],ac,comp,centre,extraPoints[2]);
            } else{
                component[i]=new Component(bodyParts[i],ac,comp,centre,null);
            }
            if(i!=HAND_LEFT_LOWER && i!=HAND_RIGHT_LOWER && i!=LEG_LEFT_LOWER && i!=LEG_RIGHT_LOWER)
                robot.add(component[i]);
            this.Points[i]=pts;
            pts=null;
            System.out.println(" ...Done");
        }
        initFace();
    }
    
    private void initWalk(){
        this.frames=new Frame[20];
        //this.frames[0]=new Frame(25,25,-25,22,-25,0,25,-25,20);
        /*this.frames[0]=new Frame(  5,  5, -5,  2, -5,  0,  5, -4,  2);
        this.frames[1]=new Frame(  5,  2, -5,  2, -5,  0,  5, -4,  5);
        this.frames[2]=new Frame(  5,  5, -2, -1, -2,  0,  5, -2,  5);
        this.frames[3]=new Frame(  5,  5, -2, -1, -2,  0,  5, -2,  5);
        this.frames[4]=new Frame( 15, 10,-10, 15,-13,  0, 10,-10, 10);
        this.frames[5]=new Frame(-15,-10, 10,-15, 13,  0,-10, 10, 10);
        this.frames[6]=new Frame( -5, -5,  2,  1,  2,  0, -5,  2,  5);
        this.frames[7]=new Frame( -5, -5,  2,  1,  2,  0, -5,  2,  5);
        this.frames[8]=new Frame( -5, -2,  5, -2,  5,  0, -5,  4,  5);
        this.frames[9]=new Frame( -6, -6,  6, -3,  6,  0, -6,  5,  2);
         
        this.frames[10]=new Frame( -5,  2,  5,  5,  5, -4, -5,  0,  2);
        this.frames[11]=new Frame( -5,  2,  5,  2,  5, -4, -5,  0,  5);
        this.frames[12]=new Frame( -2, -1,  5,  5,  5, -2, -2,  0,  5);
        this.frames[13]=new Frame( -2, -1,  5,  5,  5, -2, -2,  0,  5);
        this.frames[14]=new Frame(-10, 15, 15, 10, 10,-10,-13,  0, 10);
        this.frames[15]=new Frame( 10,-15,-15,-10,-10, 10, 13,  0, 10);
        this.frames[16]=new Frame(  2,  1, -5, -5, -5,  2,  2,  0,  5);
        this.frames[17]=new Frame(  2,  1, -5, -5, -5,  2,  2,  0,  5);
        this.frames[18]=new Frame(  5, -2, -5, -2, -5,  4,  5,  0,  5);
        this.frames[19]=new Frame(  6, -3, -6, -6, -6,  5,  6,  0,  2);
         */
        this.frames[0]=new Frame(  5,  5, -5,  2, -5,  0,  5, -4,  12);
        this.frames[1]=new Frame(  5,  2, -5,  2, -5,  0,  5, -4,  7);
        this.frames[2]=new Frame(  5,  5, -2, -1, -2,  0,  5, -2,  7);
        this.frames[3]=new Frame(  5,  5, -2, -1, -2,  0,  5, -2,  7);
        this.frames[4]=new Frame( 15, 10,-10, 15,-13,  0, 10,-10, 15);
        this.frames[5]=new Frame(-15,-10, 10,-15, 13,  0,-10, 10, 15);
        this.frames[6]=new Frame( -5, -5,  2,  1,  2,  0, -5,  2,  7);
        this.frames[7]=new Frame( -5, -5,  2,  1,  2,  0, -5,  2,  7);
        this.frames[8]=new Frame( -5, -2,  5, -2,  5,  0, -5,  4,  7);
        this.frames[9]=new Frame( -5, -5,  5, -2,  5,  0, -5,  4,  12);
        
        this.frames[10]=new Frame( -5,  2,  5,  5,  5, -4, -5,  0,  12);
        this.frames[11]=new Frame( -5,  2,  5,  2,  5, -4, -5,  0,  7);
        this.frames[12]=new Frame( -2, -1,  5,  5,  5, -2, -2,  0,  7);
        this.frames[13]=new Frame( -2, -1,  5,  5,  5, -2, -2,  0,  7);
        this.frames[14]=new Frame(-10, 15, 15, 10, 10,-10,-13,  0, 15);
        this.frames[15]=new Frame( 10,-15,-15,-10,-10, 10, 13,  0, 15);
        this.frames[16]=new Frame(  2,  1, -5, -5, -5,  2,  2,  0,  7);
        this.frames[17]=new Frame(  2,  1, -5, -5, -5,  2,  2,  0,  7);
        this.frames[18]=new Frame(  5, -2, -5, -2, -5,  4,  5,  0,  7);
        this.frames[19]=new Frame(  5, -2, -5, -5, -5,  4,  5,  0,  12);
    }
    
    public Point3D getLookPoint() {
        return this.Points[this.BODY][this.CC];
    }
    
    public void RotateY(Point3D[] pts,double ang,Point3D about){
        float tx,tz;
        float ax=about.getX(),az=about.getZ();
        ang=ang*Math.PI/180;
        System.out.println("About : "+about+"  By ang :"+ang);
        for(Point3D p: pts){
            if(p.isMoved()) continue;
            System.out.print(p.toString()+"  ==>  ");
            tx=p.getX();
            tz=p.getZ();
            p.setX((float)(tx*Math.cos(ang) -tz*Math.sin(ang) + (ax*(1-Math.cos(ang))+az*Math.sin(ang))));
            p.setZ((float)(tx*Math.sin(ang) +tz*Math.cos(ang) + (az*(1-Math.cos(ang))-ax*Math.sin(ang))));
            System.out.println(p.toString());
            p.setMoved(true);
        }
    }
    public void RotateY(Point3D p,double ang,Point3D about){
        if(p.isMoved()) return;
        float tx,tz;
        float ax=about.getX(),az=about.getZ();
        ang=ang*Math.PI/180;
        tx=p.getX();
        tz=p.getZ();
        p.setX((float)(tx*Math.cos(ang) -tz*Math.sin(ang) + (ax*(1-Math.cos(ang))+az*Math.sin(ang))));
        p.setZ((float)(tx*Math.sin(ang) +tz*Math.cos(ang) + (az*(1-Math.cos(ang))-ax*Math.sin(ang))));
        p.setMoved(true);
        
    }
    
    public void run() {
        int frame=0;
        // destroyRobot is used to stop this thread
        while(!isDestroyRobot()){
            if(isRobotWalking())    {
                try {
                    Thread.sleep(walkingSpeed);
                } catch (InterruptedException ex) {
                    ex.printStackTrace();
                }
                frame++;
                if(frame>19){
                    frame%=20;
                }
                ac.setLookPoint(getLookPoint());
                ac.refreshCanvas(frame);
            }
        }
    }
    
    public boolean isRobotWalking() {
        return robotWalking;
    }
    
    public void setRobotWalking(boolean robotWalking) {
        this.robotWalking = robotWalking;
    }
    
    public Integer getWalkingSpeed() {
        return walkingSpeed;
    }
    
    public void setWalkingSpeed(Integer walkingSpeed) {
        this.walkingSpeed = walkingSpeed;
    }
    
    private int EYE_LEFT=0,
            EYE_RIGHT=1,
            NOSE=2,
            MOUTH=3;
    private void initFace() {
        Point3D p=this.Points[this.HEAD][this.FC];
        this.FacePoints= new Point3D[4][];
        for (int i = 0; i < 4; i++) {
            this.FacePoints[i]=new Point3D[4];
        }
        this.FacePoints[EYE_LEFT][0]=new Point3D(ac,p.getX(),p.getY()-Constants.HEAD_BREDTH/3+Constants.HEAD_BREDTH/5,p.getZ()+Constants.HEAD_HEIGHT/2-Constants.HEAD_HEIGHT/6);
        this.FacePoints[EYE_LEFT][1]=new Point3D(ac,p.getX(),p.getY()-Constants.HEAD_BREDTH/3,                        p.getZ()+Constants.HEAD_HEIGHT/2-Constants.HEAD_HEIGHT/6);
        this.FacePoints[EYE_LEFT][2]=new Point3D(ac,p.getX(),p.getY()-Constants.HEAD_BREDTH/3,                        p.getZ()+Constants.HEAD_HEIGHT/2-Constants.HEAD_HEIGHT/3);
        this.FacePoints[EYE_LEFT][3]=new Point3D(ac,p.getX(),p.getY()-Constants.HEAD_BREDTH/3+Constants.HEAD_BREDTH/5,p.getZ()+Constants.HEAD_HEIGHT/2-Constants.HEAD_HEIGHT/3);
        
        this.FacePoints[EYE_RIGHT][0]=new Point3D(ac,p.getX(),p.getY()+Constants.HEAD_BREDTH/3,                        p.getZ()+Constants.HEAD_HEIGHT/2-Constants.HEAD_HEIGHT/6);
        this.FacePoints[EYE_RIGHT][1]=new Point3D(ac,p.getX(),p.getY()+Constants.HEAD_BREDTH/3-Constants.HEAD_BREDTH/5,p.getZ()+Constants.HEAD_HEIGHT/2-Constants.HEAD_HEIGHT/6);
        this.FacePoints[EYE_RIGHT][2]=new Point3D(ac,p.getX(),p.getY()+Constants.HEAD_BREDTH/3-Constants.HEAD_BREDTH/5,p.getZ()+Constants.HEAD_HEIGHT/2-Constants.HEAD_HEIGHT/3);
        this.FacePoints[EYE_RIGHT][3]=new Point3D(ac,p.getX(),p.getY()+Constants.HEAD_BREDTH/3,                        p.getZ()+Constants.HEAD_HEIGHT/2-Constants.HEAD_HEIGHT/3);
        
        this.FacePoints[NOSE][0]=new Point3D(ac,p.getX(),p.getY(),                        p.getZ()+Constants.HEAD_HEIGHT/2-Constants.HEAD_HEIGHT/6);
        this.FacePoints[NOSE][1]=new Point3D(ac,p.getX(),p.getY()-Constants.HEAD_BREDTH/9,p.getZ());
        this.FacePoints[NOSE][2]=new Point3D(ac,p.getX(),p.getY(),                        p.getZ()-Constants.HEAD_HEIGHT/15);
        this.FacePoints[NOSE][3]=new Point3D(ac,p.getX(),p.getY()+Constants.HEAD_BREDTH/9,p.getZ());
        
        this.FacePoints[MOUTH][0]=new Point3D(ac,p.getX(),p.getY()+Constants.HEAD_BREDTH/3,p.getZ()-Constants.HEAD_HEIGHT/4);
        this.FacePoints[MOUTH][1]=new Point3D(ac,p.getX(),p.getY(),                        p.getZ()-Constants.HEAD_HEIGHT/4+Constants.HEAD_HEIGHT/10);
        this.FacePoints[MOUTH][2]=new Point3D(ac,p.getX(),p.getY()-Constants.HEAD_BREDTH/3,p.getZ()-Constants.HEAD_HEIGHT/4);
        this.FacePoints[MOUTH][3]=new Point3D(ac,p.getX(),p.getY(),                        p.getZ()-Constants.HEAD_HEIGHT/4-Constants.HEAD_HEIGHT/10);
    }
    
    public void PaintFace(Graphics2D g2){
        //paint eye
        int[] dx=new int[4],dy=new int[4];
        int i;
        for(int j=0;j<4;j++){
            i=0;
            for(Point3D p: FacePoints[j]){
                dx[i]=p.getDx();
                dy[i]=p.getDy();
                i++;
            }
            g2.setColor(new Color(50,50,50));
            Polygon poly= new Polygon(dx,dy,4);
            g2.fillPolygon(poly);
            //g2.setColor(new Color(0,0,0));
            //g2.drawPolygon(poly);
        }
    }
 
    public boolean isDestroyRobot() {
        return destroyRobot;
    }

    public void setDestroyRobot(boolean destroyRobot) {
        this.destroyRobot = destroyRobot;
    }
}
